#include "Batch.h"

#include <iostream>

//#include "HelperFunctions.h"

void Batch::setBatchLimits( PO & po, int begin_frame_counter, int & batch_begin_frame, int & batch_num_of_frames )
{
	if ( begin_frame_counter + po.batch_size > po.begin_frame + po.num_of_frames ) 
	{
		batch_num_of_frames = po.begin_frame + po.num_of_frames - begin_frame_counter;
	} 
	else 
	{
		batch_num_of_frames = po.batch_size;
	}
	
	batch_begin_frame = begin_frame_counter;
	if ( !po.first_graph )
	{
		batch_begin_frame--;
		batch_num_of_frames++;
	}

	batch_end_frame = batch_begin_frame + batch_num_of_frames;
}

void Batch::getSource()
{
	prev_source = source;
	if ( sources.size() == 0 ) 
	{
		source = p_po->vsource;
	} 
	else 
	{
		location l = sources[0];
		source = (l.y-1) * p_po->pom_x +l.x;
		sources.erase(sources.begin());
	}
}

void Batch::readFile( int frame, vector<float> & proba )
{
	string frame_s = lexical_cast<string>(frame);
# ifndef USE_OLD_DAT_NOTATION
	string file_name = p_po->proba_path + frame_s + "-proba.dat";
# else
	string file_name = p_po->proba_path + "proba-f" + frame_s + ".dat";
#endif
	ifstream file(file_name.c_str());

	if(!file.is_open())
	{
		throw vbl::runtime_error( "[readFile] Proba file not found: " + file_name );
	} 
	else 
	{
		string line;
		while(getline(file, line))
		{
			// find 'space'-character, value behind it is probability, value before it is location
			for( unsigned int j = 0 ; j < line.size() ; j++ )
			{
				char c = line.at(j);
				if(c == ' ')
				{
					line = line.substr(j+1,line.size());
					break;
				}
			}

			float probability = lexical_cast<float>(line);
			proba.push_back(probability);

			float cost;
			// check for extremes, otherwise these numbers will create Inf numbers further on
			if( probability > 0.999999 ) 
			{
				cost = (float) -(log( 0.999999 / ( 1 - 0.999999 )) + offset); //+ offset);
			}
			else if( probability < 1e-6 ) 
			{
				cost = (float) -(log( 1e-6 / ( 1 - 1e-6 )) + offset); //+ offset);
			}
			else
			{
				cost = -(log( probability / ( 1 - probability )) + offset); //+ offset);
			}

			vertices.push_back(cost);
		}

		file.close();
	}
}

void Batch::readFiles()
{
	cout << "...Reading files..." << endl;
	
	boost::progress_display progress( batch_num_of_frames );
	for( int frame = batch_begin_frame ; frame < batch_end_frame ; frame++)
	{
		vector<float> proba;
		try
		{
			readFile(frame, proba);
		} catch( vbl::runtime_error e ) 
		{
			cout << e.what() << endl;
			exit(-1);
		}
		p_po->probabilities.push_back(proba);
		++progress;
	}
	cout << endl;
}

void Batch::getImagesLab( int frame, vector<ImageLab> & ims_out )
{
	vector<img::ImageRGB> ims;
	getImages( frame, ims );

	for( unsigned int j = 0 ; j < ims.size() ; j++ )
	{
		ImageLab imgOut;
		img::conv::RGB2Lab( ims[j], imgOut );
		ims_out.push_back( imgOut );
	}
}

void Batch::getImages( int frame, vector<img::ImageRGB> & ims_out )
{
	vector<img::ImageRGB> ims;
	for( int cam = 0; cam < p_po->num_of_cams ; ++cam )
	{
		if( p_po->use_png )
		{
			img::ImageRGB im;
			string filename = p_po->image_path + "c" + lexical_cast<string>(cam) + "_" +
				lexical_cast<string>(batch_begin_frame + frame) + ".png";
			img::io::read( filename, im );
			ims.push_back(im);
		}
		else
		{
			img::ImageG imRaw( p_po->image_width, p_po->image_height );
#ifndef USE_OLD_FILESYSTEM
			img::io::readRAW( p_po->files_per_camera[cam][ frame ], imRaw, imRaw.width(), imRaw.height() ); 
#else
			img::io::readRAW( p_po->image_path + "/" + p_po->files_per_camera[cam][ batch_begin_frame + frame ], imRaw, imRaw.width(), imRaw.height() ); 
#endif
			img::ImageRGB im;
			img::proc::demosaicPixelGrouping( imRaw, im );
			p_po->undistortImage( im, p_po->dist_map.first[cam], p_po->dist_map.second[cam] );
			img::rotateSafe< img::InterpolatorBilinear >( im, p_po->image_width/2, p_po->image_height/2, p_po->rot[cam] );
			ims.push_back(im);
		}
	}
	ims_out = ims;
}

void Batch::getNeighbours( int location, vector<int> & neighbours )
{
	// the location in a timestep
	int location_in_timestep = location % p_po->pom_size;

	bool top = true;
	bool bottom = true;
	bool left = true;
	bool right = true;

	// check if the location is on an edge of the room
	if( location_in_timestep % p_po->pom_x == 0 ) 
	{ 
		// node is on left side of grid
		left = false;
	}
	if( location_in_timestep % p_po->pom_x == (p_po->pom_x - 1) ) 
	{
		// node is on right side of grid
		right = false;
	}
	if( location_in_timestep < p_po->pom_x ) 
	{
		// node is on top side of grid
		top = false;
	}
	if( location_in_timestep >= ( p_po->pom_size - p_po->pom_x ) ) 
	{
		// node is on bottom side of grid
		bottom = false;
	}

	if( location > vsink - p_po->pom_size - 2 ) 
	{
		if( !left || !right || !top || !bottom ) 
		{
			neighbours.push_back( p_po->vsource );
		}
		// TODO: vsink should be total_locations
		// location is situated the last timestep,
		// so it only needs to be connected to vsink
		// "-2" because of the extra locations vsource and vsink
		neighbours.push_back( vsink );
		return;
	}

	// correct location, vsource = 0 so all locations move up by 1
	location = location + 1;
	neighbours.push_back( location + p_po->pom_size);								// same location
	if( left ) 
	{
		neighbours.push_back( location - 1 + p_po->pom_size);						// left
	}
	if( right ) 
	{
		neighbours.push_back(location + 1 + p_po->pom_size);						// right
	}
	if( top ) 
	{
		neighbours.push_back(location - p_po->pom_x + p_po->pom_size);			// top
		if( left ) 
		{
			neighbours.push_back(location - p_po->pom_x - 1 + p_po->pom_size);	// top left
		}
		if( right ) 
		{
			neighbours.push_back(location - p_po->pom_x + 1 + p_po->pom_size);	// top right
		}
	}
	if( bottom ) 
	{
		neighbours.push_back(location + p_po->pom_x + p_po->pom_size);			// bottom
		if( left ) 
		{
			neighbours.push_back(location + p_po->pom_x - 1 + p_po->pom_size);	// bottom left
		}
		if( right ) 
		{
			neighbours.push_back(location + p_po->pom_x + 1 + p_po->pom_size);	// bottom right
		}
	}
	if( !left || !right || !top || !bottom ) 
	{
			neighbours.push_back( p_po->vsource );
			neighbours.push_back( vsink );
	}
}

double Batch::getProbability( int frame, int location, unsigned int range )
{
	double probability = p_po->probabilities[frame][location]; // TODO: is frame correct over multiple batches?
	int count = 1;
	for( unsigned int i = 1 ; i <= range ; i++ )
	{
		if ( frame+i >= 0 && frame+i < p_po->probabilities.size() )
		{
			probability += p_po->probabilities[frame+i][location];
			count++;
		}
	}
	for( unsigned int i = 1 ; i <= range ; i++ )
	{
		if ( frame-i >= 0 && frame-i < p_po->probabilities.size() )
		{
			probability += p_po->probabilities[frame-i][location];
			count++;
		}
	}
	return probability/count;
}

void Batch::getAppearance( int frame, rectangle r, img::ImageRGB & im, img::ImageRGB & background, AppearanceHist & apHist )
{
	img::Triple<> maskColor(0,204,153);

	img::ImageRGB cutout( r.xmax - r.xmin, r.ymax - r.ymin );
	img::ImageRGB backgroundCutOut( r.xmax - r.xmin , r.ymax - r.ymin );

	img::copy( im, r.xmin, r.ymin, r.xmax - r.xmin, r.ymax - r.ymin, cutout);
	img::copy( background , r.xmin , r.ymin , r.xmax - r.xmin , r.ymax - r.ymin , backgroundCutOut );

	ImageLab cutoutLab;
	img::conv::RGB2Lab( cutout, cutoutLab );

	apHist.addLab( cutoutLab, backgroundCutOut, maskColor );
	apHist.normalize();
}

bool Batch::isOccluded( int frame, int location, int cam )
{
	map<int,vector<int>>::iterator it = p_po->occlusion_map[ cam ].find( location );
	if( it == p_po->occlusion_map[ cam ].end() )
	{
		//cout << "Location not in occlusion map" << endl;
		return false;
	}
	else
	{
		vector<int> occ = it->second;
		for( unsigned int i = 0 ; i < occ.size() ; i++ )
		{
			double val = p_po->probabilities[ frame - p_po->begin_frame ][ occ[i]-1 ];
			if( val > 0.5 )
				return true;
		}
		return false;
	}
}

void Batch::getAppearance2( int frame, int location, vector<img::ImageRGB> & ims, vector<AppearanceHist> & appearance,
						  vector<bool> & visibility )
{
	pair<int,int> ap_loc( frame, location );
	hist_map_t::iterator it = hist_map.find(ap_loc);
	if( it  != hist_map.end() )
	{
		appearance = it->second;
		visibility = vis_map[ ap_loc ];
	}
	else 
	{
		for( int cam = 0 ; cam < p_po->num_of_cams ; cam++ )
		{
			rectangle r = p_po->pom_locations[cam][location-1];
			if( !isOccluded( frame, location, cam ) && r.isVisible )
			{
				img::ImageRGB background;
#ifndef USE_CAS_DATASET
				string backgroundImPath = p_po->background_path + lexical_cast<string>(frame) + "_" + lexical_cast<string>(cam) + 
					"-result.png";
#else
				string backgroundImPath = p_po->background_path + "result-f" + lexical_cast<string>(frame) + "-c" + 
					lexical_cast<string>(cam) + ".png";
#endif
				try
				{
					img::io::read(backgroundImPath , background );
				} 
				catch( vbl::runtime_error e ) 
				{
					cout << " Could not read background file: " << e.what() << endl;
					exit(1);
				}
				AppearanceHist apHist;
				getAppearance( frame, r, ims[cam], background, apHist );
				appearance.push_back(apHist);
				visibility.push_back( true );
			}
			else
			{
				AppearanceHist apHist;
				appearance.push_back(apHist);
				visibility.push_back( false );
			}
		}
		hist_map[ap_loc] = appearance;
		vis_map[ ap_loc ] = visibility;
	}
}

//void Batch::calculateCostsAppearance( float cost, int frame, int location, vector<int> & neighbours, vector<img::ImageRGB> & ims1, 
//							  vector<img::ImageRGB> & ims2, vector<float> & costs )
//{
//	// get appearance
//	vector<AppearanceHist> appearance;
//	//getAppearance( frame, location, ims1, appearance );
//	
//	// for every neighbour get appearance
//	vector<vector<AppearanceHist>> neighbourAppearances;
//	for( unsigned int n = 0 ; n < neighbours.size() ; n++ )
//	{
//		if( neighbours[n] != p_po->vsource && neighbours[n] != vsink )
//		{
//			int neighbour = neighbours[n] % p_po->pom_size;
//			vector<AppearanceHist> neighbourAp;
//			//getAppearance( frame+1, neighbour, ims2, neighbourAp );
//			neighbourAppearances.push_back(neighbourAp);
//		}
//	}
//
//	// compare using bhatta
//	vector<float> distances;
//	computeBhattacharyyaCoefficients( appearance, neighbourAppearances, distances );
//
//	// reweight costs
//	reweighCosts( cost, distances, costs );
//}

void Batch::reweighCosts( float cost, vector<float> & distances, vector<float> & costs )
{
	float compareCost = distances[0];
	for( unsigned int d = 0 ; d < distances.size() ; d++ )
	{
		if( distances[d] == -1 )
		{
			costs.push_back( cost );
		} 
		else 
		{
			float scalar;
			if( cost <= 0 )
			{
				scalar = pow(((distances[d]-compareCost)/compareCost)+1,10);
			} 
			else 
			{
				scalar = pow(1-((distances[d]-compareCost)/compareCost),10);
			}
			costs.push_back( scalar * cost );
		}
	}
}

//void Batch::computeBhattacharyyaCoefficients( vector<AppearanceHist> & appearance, 
//											 vector<vector<AppearanceHist>> & neighbourAppearances, 
//											 vector<float> & distances )
//{
//	AppearanceHist combinedHist;
//	// Combine histograms from different cameras for all parts and keep them normalized
//	for( unsigned int n = 0; n < appearance.size() ; ++n )
//	{
//		combinedHist.add( appearance[n], 1.0 / (double)(n + 1) );
//	}
//
//	for( unsigned int n = 0 ; n < neighbourAppearances.size() ; ++n )
//	{
//		vector<AppearanceHist> neighbourAppearance = neighbourAppearances[n];
//		if( neighbourAppearance.size() != appearance.size() )
//		{
//			// not same ammount of visible cams
//			distances.push_back(-1);
//		}
//		else
//		{
//			AppearanceHist combinedHistN;
//			for( unsigned int na = 0; na < neighbourAppearance.size() ; ++na )
//			{
//				combinedHistN.add( neighbourAppearance[na], 1.0 / (double)(na + 1) );
//			}
//			distances.push_back( combinedHist.bhattacharyyaCoefficient( combinedHistN ));
//		}
//	}
//}

void Batch::createGraph( graph_t & g )
{
	cout << "...Creating Graph..." << endl;

	if ( p_po->first_graph && !p_po->use_alt_sources ) 
	{
		// connect vsource to all nodes in first timestep
		for( unsigned int i = 1 ; i <= p_po->pom_size ; i++ )
		{
			add_edge( p_po->vsource , i , 0 , g );
		}
	} 
	else 
	{
		for( unsigned int i = 0 ; i < sources.size() ; i++ ) 
		{
			location loc = sources[i];
			int l = (loc.y-1)*p_po->pom_x + loc.x;
			add_edge( p_po->vsource , l , 0 , g );
		}
	}
	
	//vector<ImageLab> ims1, ims2;
	
	bool frame_change = false;
	int current_frame = 0;

	boost::progress_display progress( vertices.size() );
	for( unsigned int i = 0 ; i < vertices.size() ; i++ )
	{
		++progress;
		int frame = (int) floor( ((double) i ) / p_po->pom_size );

		if( frame != current_frame )
		{
			current_frame = frame;
			frame_change = true;
		}

		float cost = vertices[i];
		vector<int> neighbours;
		getNeighbours( i, neighbours );

		vector<float> costs;
		
		int location = ( i % p_po->pom_size );
		//double probability = getProbability( batch_begin_frame - p_po->begin_frame + frame, location, 0 );
		//// TODO make threshold a variable
		//if( p_po->use_appearance && probability > 0.05 && frame != batch_num_of_frames-1 )
		//{
		//	calculateCostsAppearance( cost, frame, location, neighbours, ims1, ims2, costs );
		//}
		//else
		//{
			for( unsigned int neighbour = 0 ; neighbour < neighbours.size() ; neighbour++ )
			{
				costs.push_back(cost);
			}
		//}

		int trueNeighbourCounter = 0; // i.e. not vsource and vsink
		for( unsigned int j = 0 ; j < neighbours.size() ; j++ ) 
		{
			// TODO: ugly coding, +1 is to correct for nodes beginning at 0 (vsource == 0)
			if( neighbours[j] == p_po->vsource ) 
			{
				if ( i+1 > p_po->pom_size ) 
				{
					add_edge( p_po->vsource , i+1 , 0 , g );
				}
			} 
			else if( neighbours[j] == vsink )
			{
				add_edge( i+1 , vsink , cost , g );
			}
			else 
			{
				add_edge( i+1 , neighbours[j] , costs[trueNeighbourCounter] , g );
				trueNeighbourCounter++;
			}
		}
	}
	cout << endl;

}

void Batch::find_shortest_path_dag( graph_t & g, vector<int> & path ) 
{
	// find graph properties
	distance_map = get( vertex_distance , g );
	weight_map_t weight_map = get( edge_weight, g );
	vector<default_color_type> color(num_vertices(g));
	vector<size_t> predecessor_map(num_vertices(g));
	
	// initialize variables for dag_shortest_paths
	default_dijkstra_visitor vis;
	closed_plus<float> combine;
	less<float> compare;
	
	// perform dag_shortest_paths
	dag_shortest_paths( g , source , distance_map, weight_map, &color[0], &predecessor_map[0], vis, compare, combine, 
		(numeric_limits<float>::max)(), 0);

	// find shortest path by tracing back from vsink
	reconstructPath( predecessor_map, source, vsink, path );
}

void Batch::reconstructPath( vector<size_t> & predecessorMap , int source , int vsink , vector<int> & path ) 
{
	int v = vsink;
	while( v != source ) 
	{
		path.insert( path.begin(), v );
		int v_temp = predecessorMap[v];
		if( v == v_temp ) 
		{
			cout << "Node is its own predecessor" << endl;
			exit(0);
		} 
		else 
		{
			v = v_temp;
		}
	}
	path.insert( path.begin(), source );
	if ( source != p_po->vsource ) {
		path.insert( path.begin(), p_po->vsource );
	}
}

void Batch::find_shortest_path_dijkstra( graph_t & g, vector<int> & path ) 
{
	vector<size_t> predecessorMap(num_vertices(g));
    
	distance_map = get( vertex_distance , g );
	weight_map_t weight_map = get( edge_weight , g );
	index_map_t index_map = get( vertex_index , g );
	
	// initialize variables for dijkstra_shortest_paths
	default_dijkstra_visitor vis;
	closed_plus<double> combine;
	less<double> compare;
    
	dijkstra_shortest_paths(g, source, &predecessorMap[0], distance_map, weight_map, index_map, compare, combine, 
		(numeric_limits<float>::max)(), 0, vis);

	// find shortest path by tracing back from vsink
	reconstructPath( predecessorMap , source , vsink , path );
}

void Batch::getShortestPath( graph_t & g, vector<int> & path )
{
	path.clear();
	if( paths.size() == 0 )
	{
		find_shortest_path_dag( g, path );
	} 
	else
	{
		find_shortest_path_dijkstra( g, path );
	}
}

float Batch::pathsCost() 
{
	weight_map_t weight_map = get( edge_weight, g_original );
	float w = 0.0;
	for( unsigned int i = 0 ; i < paths.size() ; i++ )
	{
		vector<int> path = paths[i];
		float cost = pathCost( path, weight_map );
		w += cost;
		// TODO REMOVE
		//cout << "Path: " << i << ", cost: " << cost << endl;
		// END TODO
	}
	return w;
}

float Batch::pathCost( vector<int> & path, weight_map_t & weight_map ) 
{
	float w = 0.0;

	for( unsigned int j = 0 ; j < path.size() ; j++ ) 
	{
		if( j != 0 ) 
		{
			edge_t e;
			bool found;
			tie(e, found) = edge(path[j-1],path[j], g_original );
			if( found )
			{
				float weight = get(weight_map, e);
				w += weight;
			}
			else
			{
				cerr << "Unable to find edge to calculate costs." << endl;
				exit(-1);
			}
		}
	}

	return w;
}

void Batch::copy_weight_and_distance_map( graph_t & g , map<pair<int, int>,double> & weights , distance_map_t & distance_map, 
								  vector_property_map<double> & distances ) 
{
	weight_map_t weight_map = get( edge_weight, g );
	pair<vertex_iter, vertex_iter> vi;
	for ( vi = boost::vertices( g ) ; vi.first != vi.second ; vi.first++ ) 
	{
		distances[*vi.first] = distance_map[*vi.first];
		graph_traits<graph_t>::out_edge_iterator out_it, out_it_end;
		tie( out_it, out_it_end ) = out_edges( *vi.first , g );
		for ( ; out_it != out_it_end ; out_it++ ) 
		{
			vertex_t t = target( *out_it , g );
			double w = get(weight_map, *out_it);
			pair<int, int> key(*vi.first,t);
			weights.insert(make_pair(key, w));
		}
	}	
}

void Batch::extendGraph( graph_t & g , map<int,int> & vertexPairs ) 
{
	for ( unsigned int i = 0 ; i < paths.size() ; i++ ) 
	{
		extendGraphPath( g , paths[i] , vertexPairs );
	}
}

void Batch::extendGraphPath( graph_t & g , vector<int> & path , map<int,int> & vertexPairs ) 
{
	weight_map_t weight_map = get( edge_weight , g );
	vector<int> new_path;

	new_path.push_back( path[0] );

	for ( unsigned int i = 0 ; i < path.size() ; i++ ) 
	{
		if ( path[i] != p_po->vsource && path[i] != vsink ) 
		{
			int loc = path[i];
			
			if( loc > vsink || ( i < path.size()-1 && path[i+1] > vsink) )
			{
				if( vertexPairs[loc] == path[i+1] )
				{
					new_path.push_back( path[i+1] );
					// merge nodes
					remove_edge( loc, path[i+1], g );
					
					edge_t e;
					bool found;
					tie( e , found ) = edge( path[i-1], loc, g );
					float cost = get( edge_weight , g , e );
					remove_edge( path[i-1], loc, g );
					add_edge( path[i-1], path[i+1], cost, g);

					graph_traits<graph_t>::out_edge_iterator ei, ei_end;
					for( tie(ei, ei_end) = out_edges( loc, g ) ; ei != ei_end; ++ei) 
					{
						cost = get( edge_weight , g , *ei );
						add_edge( path[i+1], target( *ei , g ), cost, g);
					}

					//remove_vertex( loc, g );
					vertexPairs.erase( loc );
					i++;
				}
				else
				{
					new_path.push_back( loc );
				}
			}
			else
			{
				new_path.push_back( loc );
				// add new vertex
				vertex_t v = add_vertex( g );
				new_path.push_back( v );
				vertexPairs.insert(make_pair(v,loc));
				// connect all out edges of path[i] to new vertex
				graph_traits<graph_t>::out_edge_iterator out_it, out_it_end;
				tie( out_it, out_it_end ) = out_edges( loc , g );
				vector<vertex_t> toRemove;
				
				for ( ; out_it != out_it_end ; out_it++ ) 
				{
					float cost = get( weight_map , *out_it );
					vertex_t t = target( *out_it , g );
					add_edge( v , t , cost, g );
					toRemove.push_back( t );
				}
				
				// remove all out edges from path[i]
				for ( unsigned int j = 0 ; j < toRemove.size() ; j++ ) 
				{
					remove_edge( loc , toRemove[j] , g );
				}
				
				// connect path[i] to new vertex with cost 0
				add_edge( loc , v , 0 , g );
			}
		}
	}

	new_path.push_back( path[path.size()-1] );

	// for all vertices in the path, reverse direction and negate cost
	for ( unsigned int i = 1 ; i < new_path.size() ; i++ ) 
	{
		edge_t e;
		bool found;
		tie( e , found ) = edge( new_path[i-1] , new_path[i], g );
		if( found ) 
		{
			float cost = get( weight_map , e );
			remove_edge( new_path[i-1] , new_path[i], g );
			add_edge( new_path[i] , new_path[i-1] , -cost , g );
		}
		else 
		{
			cout << "Edge not found while reversing cost and direction of a path" << endl;
			cout << "Edge going from " << new_path[i-1] << " to " << new_path[i] << endl;
		}
	}
}

void Batch::transformEdgeCost( graph_t & g )//map<pair<int,int>, double> & weights_in, vector_property_map<double> & distance_map_in, 
						 //map<int,int> & vertexPairs ) 
{
	//if( prev_source != p_po->vsource )
	//{
		edge_iter ei, ei_end;
		tie( ei , ei_end ) = edges( g );
		for( ; ei != ei_end ; ei++ )
		{
			vertex_t s = boost::source( *ei , g );
			vertex_t t = boost::target( *ei , g );

			float distance_s = distance_map[s];
			float distance_t = distance_map[t];

			float cost = get( edge_weight , g , *ei );

			/*if( distance_s != LONG_MAX )
			{
				if ( t >= batch_locations )
				{
					distance_t = distance_s + cost;
					distance_map[t] = distance_t;
				}
				
				if ( distance_t > distance_s + cost )
				{
					distance_t = distance_s + cost;
					distance_map[t] = distance_t;
				}*/
						

				float new_cost = cost + distance_s - distance_t;
				if( (new_cost < 0 && new_cost > -0.001) || (new_cost > 0 && new_cost < 0.001) )
					new_cost = 0;

				put( edge_weight, g, *ei, new_cost );
			/*}
			else
			{
				put( edge_weight, g, *ei, LONG_MAX );
			}*/

		}
	//}
	/*else
	{
		edge_iter ei, ei_end;
		tie( ei , ei_end ) = edges( g );
		for( ; ei != ei_end ; ei++ ) 
		{
			vertex_t s = boost::source( *ei , g );
			vertex_t t = boost::target( *ei , g );

			pair<int,int> key(s,t);
			map<pair<int,int>, double>::iterator wi = weights_in.find(key);
			double cost;
			if( wi != weights_in.end() ) 
			{
				cost = (*wi).second;
			} 
			else 
			{
				if( t == 0 ) 
				{
					pair<int,int> key(t,s);
					cost = -weights_in[key];
				} 
				else if( t >= p_po->total_locations ) 
				{
					vertex_t pairedVertex = vertexPairs[t];
					pair<int,int> key(pairedVertex,s);
					cost = -weights_in[key];
				} 
				else if( s >= p_po->total_locations ) 
				{
					vertex_t pairedVertex = vertexPairs[s];
					if ( pairedVertex == t ) 
					{
						cost = 0;
					} 
					else 
					{
						pair<int,int> key(pairedVertex,t);
						cost = weights_in[key];
					}
				} 
				else 
				{
					cost = get( edge_weight , g , *ei );
				}
			}

			double distanceSource;
			double distanceTarget;

			distanceSource = distance_map_in[s];
	 
			if ( t >= p_po->total_locations ) 
			{
				distanceTarget = distanceSource + cost;
				distance_map_in[t] = distanceTarget;
			} 
			else 
			{
				distanceTarget = distance_map_in[t];
			}
			
			if ( distanceTarget > distanceSource + cost ) 
			{
				distanceTarget = distanceSource + cost;
				distance_map_in[t] = distanceTarget;
			}
			
			double new_cost = cost + distanceSource - distanceTarget;
			if( new_cost < 0 && new_cost > -0.001 ) 
			{
				new_cost = 0;
			}
			put( edge_weight , g , *ei , new_cost );
		}
	}*/
}

void Batch::find_and_remove_competing_path( int vertex , unsigned int & continueLocation, vector<int> & toReturn )
{
	int path_location;
	for( unsigned int i = 0 ; i < paths.size() ; i++ ) 
	{
		vector<int> path = paths[i];
		for( unsigned int j = 0 ; j < path.size() ; j++ ) 
		{
			int toCompare = path[j];
			if( vertex == toCompare ) 
			{
				toReturn = paths[i];
				path_location = i;
				continueLocation = j;
				break;
			}
		}
	}
	paths.erase(paths.begin() + path_location);
}

int Batch::locationOf( int toSearch, vector<int> path ) 
{
	for( unsigned int i = 0 ; i < path.size() ; i++ ) 
	{
		if( path[i] == toSearch ) 
		{
			return i;
		}
	}
	return -1;
}

void Batch::find_leave_point( vector<int> & path, vector<int> & competing_path, int hit_location, signed int & leave_location, 
								signed int & split_location )
{
	split_location = locationOf( path[hit_location] , competing_path );
	while( true ) 
	{
		split_location -= 1;
		leave_location = locationOf( competing_path[split_location] , path );
		if ( leave_location == -1 ) 
		{
			leave_location = locationOf( competing_path[split_location+1] , path ) + 2;
			break;
		}
	}
}

void Batch::interlace_and_augment( vector<int> & path ) 
{
	// applies to path
	unsigned int hit_location = 0;
	signed int leave_location = 0;
	// applies to crossing path
	signed int split_location = 0;
	unsigned int continueLocation = 0;
	vector<int> competing_path;
	bool interlace_found = false;
	for( unsigned int i = 0 ; i < path.size() ; i++ ) 
	{
		if( i != 0 ) 
		{
			edge_t e;
			bool found;
			tie(e, found) = edge(path[i-1],path[i], g_original );
			if( !found ) 
			{
				//cout << "Interlace pos found: " << path[i-1] % 2280 << " to " << path[i] << endl;
				// not in orignal graph, so must be one of the edges going backwards
				hit_location = i-1;

				find_and_remove_competing_path( path[hit_location] , continueLocation, competing_path );
				interlace_found	= true;
				find_leave_point( path , competing_path , hit_location , leave_location , split_location );

				/*cout << "Orig Path: " << endl;
				for( int j = 0 ; j < path.size() ; j++ )
				{
					cout << path[j] % 2280 << " ";
				}
				cout << endl;

				cout << "Comp Path: " << endl;
				for( int j = 0 ; j < competing_path.size() ; j++ )
				{
					cout << competing_path[j] % 2280 << " ";
				}
				cout << endl;

				cout << "hit: " << hit_location << endl;
				cout << "leave: " << leave_location << endl;
				cout << "split: " << split_location << endl;
				cout << "continue: " << continueLocation << endl;*/

				break;
			}
		}
	}
	if( interlace_found ) 
	{
		vector<int> new_path_1;
		vector<int> new_path_2;
		for( unsigned int i = 0 ; i < hit_location ; i++ ) 
		{
			new_path_1.push_back(path[i]);
		}
		for( unsigned int i = continueLocation ; i < competing_path.size() ; i++ )
		{
			new_path_1.push_back(competing_path[i]);
		}
		for( unsigned int i = 0 ; i <= split_location ; i++ ) 
		{
			new_path_2.push_back(competing_path[i]);
		}
		for( unsigned int i = leave_location ; i < path.size() ; i++ )
		{
			new_path_2.push_back(path[i]);
		}
		paths.push_back( new_path_1 );
		interlace_and_augment( new_path_2 );
	}
	else 
	{
		paths.push_back( path );
	}
}

void put( int key, int value, map<int,vector<int>> & map_ )
{
	vector<int> values;
	map<int,vector<int>>::iterator it = map_.find(key);
	if( it != map_.end() )
	{
		values = it->second;
	}
	values.push_back( value );
	map_[ key ] = values;
}

void put( pair<int,int> key, pair<int,int> value, map<pair<int,int>,vector<pair<int,int>>> & map_ )
{
	vector<pair<int,int>> values;
	map<pair<int,int>,vector<pair<int,int>>>::iterator it = map_.find(key);
	if( it != map_.end() )
	{
		values = it->second;
	}
	values.push_back( value );
	map_[ key ] = values;
}

int Batch::intToTimestep( int i )
{
	return (int) floor( ((double) i-1) / p_po->pom_size );
}

pair<int,int> Batch::intToXY( int i )
{
	i = ( ( i - 1 ) % p_po->pom_size ) + 1;
	int locx = ((i-1) % p_po->pom_x )+1;
	int locy = (( i - locx ) / p_po->pom_x )+1;
	return make_pair(locx, locy);
}

bool Batch::withinRange( int range, int loc, int compare, pair<int,int> & diff )
{
	int ts = intToTimestep(loc);
	int ts_comp = intToTimestep( compare );
	if( ts != ts_comp )
	{
		return false;
	}

	pair<int,int> locXY = intToXY( loc );
	pair<int,int> locXY_comp = intToXY( compare );

	int x_diff = locXY_comp.first - locXY.first;
	int y_diff = locXY_comp.second - locXY.second;

	if( abs(x_diff) + abs(y_diff) <= range )
	{
		diff = make_pair( x_diff, y_diff );
		return true;
	}
	else
	{
		return false;
	}
}

void addVertex( int vertex, vector<int> & vertices, map<int, bool> & availableVertices )
{
	map<int, bool>::iterator it = availableVertices.find(vertex);
	if( it != availableVertices.end() )
	{
		return;
	}
	else
	{
		vertices.push_back( vertex );
		availableVertices[vertex] = true;
	}
}

void Batch::getVerticesInRange( int vertex, pair<int,int> diff, vector<int> & extraVertices, 
								map<int, bool> & availableVertices )
{
	if( diff.first == 0 ) // difference is in y direction
	{
		if( diff.second < 0 )
		{
			for( int i = 0 ; i >= diff.second ; i-- )
			{
				addVertex( vertex + i*p_po->pom_x, extraVertices, availableVertices );
			}
		}
		else
		{
			for( int i = 0 ; i <= diff.second ; i++ )
			{
				addVertex( vertex + i*p_po->pom_x, extraVertices, availableVertices );
			}
		}
	}
	else if( diff.second == 0 ) // difference is in x direction
	{
		if( diff.first < 0 )
		{
			for( int i = 0 ; i >= diff.first ; i-- )
			{
				addVertex( vertex + i, extraVertices, availableVertices );
			}
		}
		else
		{
			for( int i = 0 ; i <= diff.first ; i++ )
			{
				addVertex( vertex + i, extraVertices, availableVertices );
			}
		}
	}
	else // both x and y directions are non-zero
	{
		if( diff.first < 0 && diff.second < 0 )
		{
			for( int i = 0 ; i >= diff.first ; i-- )
			{
				for( int j = 0 ; j >= diff.second ; j-- )
				{
					addVertex( vertex + i + j*p_po->pom_x, extraVertices, availableVertices );
				}
			}
		}
		else if( diff.first > 0 && diff.second < 0 )
		{
			for( int i = 0 ; i <= diff.first ; i++ )
			{
				for( int j = 0 ; j >= diff.second ; j-- )
				{
					addVertex( vertex + i + j*p_po->pom_x, extraVertices, availableVertices );
				}
			}
		}
		else if( diff.first < 0 && diff.second > 0 )
		{
			for( int i = 0 ; i >= diff.first ; i-- )
			{
				for( int j = 0 ; j <= diff.second ; j++ )
				{
					addVertex( vertex + i + j*p_po->pom_x, extraVertices, availableVertices );
				}
			}
		}
		else // both larger than 0
		{
			for( int i = 0 ; i <= diff.first ; i++ )
			{
				for( int j = 0 ; j <= diff.second ; j++ )
				{
					addVertex( vertex + i + j*p_po->pom_x, extraVertices, availableVertices );
				}
			}
		}
	}
}

void Batch::findVertices( vector<int> & LP_vertices, map<int, bool> & availableVertices )
{
	for( unsigned int i = 0 ; i < paths.size() ; i++ )
	{
		vector<int> path = paths[i];
		for( unsigned int j = 1 ; j < path.size()-1 ; j++ )
		{
			int vertex = path[j];
			addVertex( vertex, LP_vertices, availableVertices );
			for( unsigned int k = 0 ; k < paths.size() ; k++ )
			{
				if ( k != i )
				{
					vector<int> comparisonPath = paths[k];
					for( unsigned int l = 1 ; l < comparisonPath.size()-1 ; l++ )
					{
						pair<int,int> diff;
						if( withinRange( 3, vertex, comparisonPath[l], diff ) )
						{
							getVerticesInRange( vertex, diff, LP_vertices, availableVertices );
						}
					}
				}
			}
		}
	}
	/*for( int i = 0 ; i < paths.size() ; i++ )
	{
		vector<int> path = paths[i];
		for( int j = 0 ; j < path.size() ; j++ )
		{
			cout << path[j] << ", ";
		}
		cout << endl;
	}
	cout << endl;
	for( int i = 0 ; i < extraVertices.size() ; i++ )
	{
		cout << extraVertices[i] << ", ";
	}
	cout << endl;*/
}

bool isAvailable( int i, map<int, bool> m )
{
	map<int, bool>::iterator it = m.find(i);
	if( it != m.end() )
	{
		return true;
	}
	else
	{
		return false;
	}
}

void Batch::getObjectAppearance( int location, vector<AppearanceHist> & app, vector<bool> & cameraVisObj, 
								vector<img::ImageRGB> & ims )
{
	int time_step = intToTimestep( location );
	int frame_number = batch_begin_frame + time_step;
	int location_in_timestep = location % p_po->pom_size;

	getAppearance2( frame_number, location_in_timestep, ims, app, cameraVisObj );
}

double KullbackLieblerDivergence( AppearanceHist t, AppearanceHist o )
{
	// TODO: check if these values are as expected
	double toReturn = 0;
	AppearanceHist::iterator t_it = t.begin();
	AppearanceHist::iterator t_it_end = t.end();
	AppearanceHist::iterator o_it = o.begin();
	for( t_it ; t_it != t_it_end ; t_it++, o_it++ )
	{
		if( *t_it > 0 && *o_it > 0 )
		{
			toReturn += ((*t_it) * log( (*t_it/ *o_it) ));
		}
	}
	return toReturn;
}

double Batch::getAppearanceValue( int group, int location, vector<img::ImageRGB> & ims )
{
	if( group >= p_po->templates.size() )
	{
		return (1.0 / (double) paths.size());
	}

	vector<AppearanceHist> templateApp = p_po->templates[group];
	vector<AppearanceHist> objectApp;
	vector<bool> visibility;
	getObjectAppearance( location, objectApp, visibility, ims );

	double sum = 0;
	int view_count = 0;
	for( unsigned int i = 0 ; i < templateApp.size() ; i++ )
	{
		if( p_po->camera_visibility[group][i] && visibility[i] )
		{
			sum += exp( -KullbackLieblerDivergence( templateApp[i], objectApp[i] ));
			view_count++;
		}
	}
	if( view_count == 0 )
	{
		return (1.0 / (double) paths.size());
	}
	else
	{
		return sum / view_count;
	}
}

int Batch::locationToInt( location l )
{
	return (p_po->pom_x * (l.y-1)) + l.x;
}

void Batch::shitrit( vector<pair<int,int>> & sources_group )
{
	cout << "[LP] Performing Shitrit appearance" << endl;
	unsigned int num_of_groups = paths.size(); // TODO: Set num_of_groups somewhere else
	if( num_of_groups < p_po->templates.size() )
		num_of_groups = p_po->templates.size();

	int max_per_group = 1;

	/*cout << "[LP] Collecting Images" << endl;
	vector<vector<ImageLab>> ims;
	for( int i = 0 ; i < batch_num_of_frames ; i++ )
	{
		cout << i << endl;
		vector<ImageLab> toAdd;
		getImagesLab( i, toAdd );
		ims.push_back( toAdd );
	}*/

/*	
	vector<tuple<int,int,int>> edges; // done
	vector<pair<int,int>> sub_vertices;	
	vector<int> sub_vertices_no_group; 
	map<pair<int,int>,vector<pair<int,int>>> ins, outs; // done
	map<tuple<int,int,int>, int> edge_map; // done
	map<int, tuple<int,int,int>> edge_map_reverse; // done
	vector<vector<vector<tuple<int,int,int>>>> edge_per_timestep;
*/

	cout << "[LP] Collecting edge and vertex information" << endl;
	vector<int> LP_vertices;
	vector<pair<int,int>> LP_vertices_group;
	//vector<tuple<int,int,int>> LP_edges;
	map<pair<int,int>,vector<pair<int,int>>> LP_in_edges, LP_out_edges;
	map<tuple<int,int,int>, int> LP_edge_map;
	map<int, tuple<int,int,int>> LP_edge_map_reverse;
	vector<vector<vector<tuple<int,int,int>>>> LP_edges_per_ts_per_group;
	map<int, bool> availability_map;

	for( int i = 0 ; i < batch_num_of_frames ; i++ )
	{
		vector<vector<tuple<int,int,int>>> toAdd;
		LP_edges_per_ts_per_group.push_back( toAdd );
		for( int j = 0 ; j < num_of_groups ; j++ )
		{
			vector<tuple<int,int,int>> toAdd2;
			LP_edges_per_ts_per_group[i].push_back( toAdd2 );
		}
	}

	int edge_counter = 1;

	findVertices( LP_vertices, availability_map );
	//addVertex( vsink, LP_vertices, availability_map );

	for( unsigned int i = 0 ; i < LP_vertices.size() ; i++ )
	{
		int v = LP_vertices[i];
		int time_step = intToTimestep( v );

		vector<int> neighbours;
		getNeighbours( v-1, neighbours );

		for( unsigned int group = 0 ; group < num_of_groups ; group++ )
		{	
			pair<int,int> gv( group, v );

			if( time_step == 0 )
			{
				tuple<int,int,int> edge( group, p_po->vsource, v );
				pair<int,int> gn( -1, p_po->vsource );
				//LP_edges.push_back( edge );
				put( gn, gv, LP_out_edges);
				put( gv, gn, LP_in_edges);
				LP_edge_map[edge] = edge_counter;
				LP_edge_map_reverse[edge_counter] = edge;
				edge_counter++;
			}

			// create same vertex per group
			LP_vertices_group.push_back( make_pair( group, v ));

			// create edge to every neighbour, per group
			for( unsigned int n = 0 ; n < neighbours.size() ; n++ )
			{
				if( isAvailable( neighbours[n], availability_map ))
				{
					tuple<int,int,int> edge( group, v, neighbours[n] );
					pair<int,int> gn( group, neighbours[n] );
					//LP_edges.push_back( edge );
					put( gv, gn, LP_out_edges);
					put( gn, gv, LP_in_edges);
					LP_edge_map[edge] = edge_counter;
					LP_edge_map_reverse[edge_counter] = edge;
					LP_edges_per_ts_per_group[ time_step ][ group ].push_back(edge);
					edge_counter++;
				}
				else if( neighbours[n] == vsink )
				{
					tuple<int,int,int> edge( group, v, vsink );
					pair<int,int> gn( -1, vsink );
					//LP_edges.push_back( edge );
					put( gv, gn, LP_out_edges);
					put( gn, gv, LP_in_edges);
					LP_edge_map[edge] = edge_counter;
					LP_edge_map_reverse[edge_counter] = edge;
					//lp_edges_per_ts_per_group[ time_step ][ group ].push_back(edge);
					edge_counter++;
				}
				else if( neighbours[n] == p_po->vsource && time_step != 0)
				{
					tuple<int,int,int> edge( group, p_po->vsource, v );
					pair<int,int> gn( -1, p_po->vsource );
					//LP_edges.push_back( edge );
					put( gn, gv, LP_out_edges);
					put( gv, gn, LP_in_edges);
					LP_edge_map[edge] = edge_counter;
					LP_edge_map_reverse[edge_counter] = edge;
					edge_counter++;
				}
			}
		}
	}

	lprec *lp;
 	int Ncol, *colno = NULL, ret = 0;
 	REAL *row = NULL;

	Ncol = edge_counter-1;

	lp = make_lp(0, Ncol);

	// objective function
	cout << "[LP] Objective Function" << endl;
	int counter = 0;
	colno = (int *) malloc(Ncol * sizeof(*colno));
	row = (REAL *) malloc(Ncol * sizeof(*row));
	
	for( unsigned int i = 0 ; i < LP_edges_per_ts_per_group.size() ; i++ )
	{
		vector<img::ImageRGB> ims;
		getImages( i, ims );
		for( unsigned int j = 0 ; j < LP_edges_per_ts_per_group[i].size() ; j++ )
		{
			for( unsigned int k = 0 ; k < LP_edges_per_ts_per_group[i][j].size() ; k++ )
			{
				tuple<int,int,int> edge = LP_edges_per_ts_per_group[i][j][k];
				if( edge.get<1>() != p_po->vsource )
				{
					int edge_i = LP_edge_map[edge];
					colno[counter] = edge_i;
					float value = -vertices[edge.get<1>()-1];
					double appValue = getAppearanceValue( edge.get<0>(), edge.get<1>(), ims );

					if( appValue == 0 )
						appValue = 0.000001;

					row[counter++] = value+log(appValue * num_of_groups);
				}
			}
		}
	}
	//for( int i = 0 ; i < LP_edges.size() ; i++ )
	//{
	//	//vector<pair<int,int>> out_vec = outs[sub_vertices[i]];
	//	//for( int j = 0 ; j < out_vec.size() ; j++ )
	//	//{
	//	//	tuple<int,int,int> edge( sub_vertices[i].first, sub_vertices[i].second, out_vec[j].second );
	//	tuple<int,int,int> edge = LP_edges[i];
	//	if( edge.get<1>() != p_po->vsource )
	//	{
	//		int edge_i = LP_edge_map[edge];
	//		colno[counter] = edge_i;
	//		float value = -vertices[edge.get<1>()-1];
	//		double appValue = getAppearanceValue( edge.get<0>(), edge.get<1>() );

	//		if( appValue == 0 )
	//			appValue = 0.000001;

	//		row[counter++] = value+log(appValue * num_of_groups);
	//	}
	//	++progress;
	//	//}
	//}
	if(!set_obj_fnex(lp, counter, row, colno))
		ret = 1;
	//cout << "Ret: " << ret << endl;

	free(colno);
	free(row);

	set_add_rowmode(lp, TRUE);

	// NOTE: THIS IS OWN ADDITION, BATCH MODE CONSISTENCY
	cout << "[LP] [Own Addition] Batch Consistency" << endl;
	if( sources_backup.size() != 0 )
	{		
		if( sources_group.size() == 0 )
		{
			for( unsigned int i = 0 ; i < sources_backup.size() ; i++ )
			{
				counter = 0;
				colno = (int *) malloc(Ncol * sizeof(*colno));
				row = (REAL *) malloc(Ncol * sizeof(*row));
				for( unsigned int group = 0 ; group < num_of_groups ; group++ )
				{
					int loc = locationToInt(sources_backup[i]);
					pair<int,int> s = make_pair( group, loc );
					vector<pair<int,int>> out_vec = LP_out_edges[s];
					for( unsigned int j = 0 ; j < out_vec.size() ; j++ )
					{
						tuple<int,int,int> edge( group, loc, out_vec[j].second );
						int edge_i = LP_edge_map[edge];
						colno[counter] = edge_i;
						row[counter++] = 1;
					}
				}
				if(!add_constraintex( lp, counter, row, colno, EQ, 1))
					ret = 12;
				free(colno);
				free(row);
			}
		}
		else
		{
			for( unsigned int i = 0 ; i < sources_group.size() ; i++ )
			{
				counter = 0;
				colno = (int *) malloc(Ncol * sizeof(*colno));
				row = (REAL *) malloc(Ncol * sizeof(*row));
				int group = sources_group[i].first;
				if( group < num_of_groups )
				{
					int loc = sources_group[i].second;
					pair<int,int> s = make_pair( group, loc );
					vector<pair<int,int>> out_vec = LP_out_edges[s];
					for( unsigned int j = 0 ; j < out_vec.size() ; j++ )
					{
						tuple<int,int,int> edge( group, loc, out_vec[j].second );
						int edge_i = LP_edge_map[edge];
						colno[counter] = edge_i;
						row[counter++] = 1;
					}
					if(!add_constraintex( lp, counter, row, colno, EQ, 1))
						ret = 12;
				}
				free(colno);
				free(row);
			}
		}
	}
	//cout << "Ret: " << ret << endl;

	// f <= 1
	cout << "[LP] f <= 1" << endl;
	for( unsigned int i = 0 ; i < LP_vertices.size() ; i++ )
	{
		counter = 0;
		colno = (int *) malloc(Ncol * sizeof(*colno));
		row = (REAL *) malloc(Ncol * sizeof(*row));
		for( unsigned int group = 0 ; group < num_of_groups ; group++ )
		{
			pair<int,int> vertex( group, LP_vertices[i] );
			vector<pair<int,int>> out_vec = LP_out_edges[vertex];
			for( unsigned int j = 0 ; j < out_vec.size() ; j++ )
			{
				tuple<int,int,int> edge( group, vertex.second, out_vec[j].second );
				int edge_i = LP_edge_map[edge];
				colno[counter] = edge_i;
				row[counter++] = 1;
			}
		}
		if(!add_constraintex( lp, counter, row, colno, LE, 1))
			ret = 2;
		free(colno);
		free(row);
	}
	//cout << "Ret: " << ret << endl;

	// f >= 0
	cout << "[LP] f >= 0" << endl;
	colno = (int *) malloc(Ncol * sizeof(*colno));
    row = (REAL *) malloc(Ncol * sizeof(*row));
	for( int i = 1 ; i <= edge_counter-1 ; i++ )
	{
		colno[0] = i;
		row[0] = 1;
		if(!add_constraintex( lp, 1, row, colno, GE, 0))
			ret = 3;
	}
	free(colno);
	free(row);
	//cout << "Ret: " << ret << endl;
	
	//	in - out <= 0
	cout << "[LP] in - out <= 0" << endl;
	for( unsigned int i = 0 ; i < LP_vertices_group.size() ; i++ )
	{
		colno = (int *) malloc(Ncol * sizeof(*colno));
		row = (REAL *) malloc(Ncol * sizeof(*row));
		counter = 0;
		vector<pair<int,int>> out_vec = LP_out_edges[LP_vertices_group[i]];
		for( unsigned int j = 0 ; j < out_vec.size() ; j++ )
		{
			tuple<int,int,int> edge( LP_vertices_group[i].first, LP_vertices_group[i].second, out_vec[j].second );
			int edge_i = LP_edge_map[edge];
			colno[counter] = edge_i;
			row[counter++] = 1;
		}
		
		vector<pair<int,int>> in_vec = LP_in_edges[LP_vertices_group[i]];
		for( unsigned int j = 0 ; j < in_vec.size() ; j++ )
		{
			tuple<int,int,int> edge( LP_vertices_group[i].first, in_vec[j].second, LP_vertices_group[i].second );
			int edge_i = LP_edge_map[edge];
			colno[counter] = edge_i;
			row[counter++] = -1;
		}
		if(!add_constraintex( lp, counter, row, colno, LE, 0))
			ret = 4;
		free(colno);
		free(row);
	}
	//cout << "Ret: " << ret << endl;

	// vsource - vsink <= 0
	cout << "[LP] vsource - vsink <= 0" << endl;
	colno = (int *) malloc(Ncol * sizeof(*colno));
	row = (REAL *) malloc(Ncol * sizeof(*row));
	counter = 0;
	vector<pair<int,int>> out_vec = LP_out_edges[make_pair(-1,p_po->vsource)];
	for( unsigned int j = 0 ; j < out_vec.size() ; j++ )
	{
		tuple<int,int,int> edge( out_vec[j].first, p_po->vsource, out_vec[j].second );
		int edge_i = LP_edge_map[edge];
		colno[counter] = edge_i;
		row[counter++] = 1;
	}
	
	vector<pair<int,int>> in_vec = LP_in_edges[make_pair(-1,vsink)];
	for( unsigned int j = 0 ; j < in_vec.size() ; j++ )
	{
		tuple<int,int,int> edge( in_vec[j].first, in_vec[j].second, vsink );
		int edge_i = LP_edge_map[edge];
		colno[counter] = edge_i;
		row[counter++] = -1;
	}
	if(!add_constraintex( lp, counter, row, colno, LE, 0))
		ret = 5;
	free(colno);
	free(row);
	//cout << "Ret: " << ret << endl;

	// f < Nl
	cout << "[LP] f < Nl" << endl;
	for( unsigned int i = 0 ; i < LP_edges_per_ts_per_group.size() ; i++ )
	{
		vector<vector<tuple<int,int,int>>> edge_one_timestep = LP_edges_per_ts_per_group[i];
		for( unsigned int j = 0 ; j < edge_one_timestep.size() ; j++ )
		{
			colno = (int *) malloc(Ncol * sizeof(*colno));
			row = (REAL *) malloc(Ncol * sizeof(*row));
			counter = 0;
			vector<tuple<int,int,int>> edge_one_group = edge_one_timestep[j];
			for( unsigned int k = 0 ; k < edge_one_group.size() ; k++ )
			{
				tuple<int,int,int> edge = edge_one_group[k];
				int edge_i = LP_edge_map[edge];
				colno[counter] = edge_i;
				row[counter++] = 1;
			}
			if(!add_constraintex( lp, counter, row, colno, LE, max_per_group))
				ret = 6;
			free(colno);
			free(row);
		}
	}
	//cout << ret << endl;

	set_add_rowmode(lp, FALSE);

	set_maxim( lp );
	//set_verbose( lp, NEUTRAL );

	cout << "[LP] Solving LP" << endl;
	ret = solve(lp);

	row = (REAL *) malloc(Ncol * sizeof(*row));
	get_variables(lp, row);

	cout << "[LP] Collecting results" << endl;
	map<pair<int,int>, int> result_map;
	vector<pair<int,int>> starting_points;
	for( int j = 0 ; j < Ncol ; j++ )
	{
		if( row[j] > 0 )
		{
			tuple<int,int,int> edge = LP_edge_map_reverse[j+1];
			if( edge.get<1>() != p_po->vsource )
			{
				pair<int,int> group_begin( edge.get<0>(), edge.get<1>() );
				result_map[group_begin] = edge.get<2>();
			}
			else
			{
				pair<int,int> group_begin( edge.get<0>(), edge.get<2>() );
				starting_points.push_back( group_begin );
			}
		}
	}
	
	sources_group.clear();

	cout << "[LP] Creating New Paths" << endl;
	for( unsigned int i = 0 ; i < starting_points.size() ; i++ )
	{
		vector<int> path;
		path.push_back(0);
		pair<int,int> group_begin = starting_points[i];
		while( group_begin.second != vsink )
		{
			path.push_back(group_begin.second);
			int end = result_map[group_begin];
			group_begin = make_pair( group_begin.first, end );
		}
		path.push_back(vsink);
		paths_shitrit.push_back(path);
		sources_group.push_back( make_pair(group_begin.first, path[path.size()-2]) );

		/*cout << "group: " << group_begin.first << " :: ";
		for( int j = 0 ; j < path.size() ; j++ )
		{
			cout << path[j] << ", ";
		}
		cout << endl;*/
	}

	if(lp != NULL) {
 	  /* clean up such that all used memory by lpsolve is freed */
 	  delete_lp(lp);
 	}
	cout << "[LP] End LP" << endl;
	cout << endl;
}

void Batch::setWeightAndDistances( graph_t & g )
{
	cout << "Performing setWeightAndDistances" << endl;
	// find graph properties
	weight_map_t weight_map = get( edge_weight, g );
	vector<default_color_type> color(num_vertices(g));
	vector<size_t> predecessor_map(num_vertices(g));
	
	// initialize variables for dag_shortest_paths
	//plus<double> combine;
	//less<double> compare;
	
	// perform dag_shortest_paths
	//bellman_ford_shortest_paths( g, source, num_vertices(g), weight_map, &predecessor_map[0], distance_map, combine, compare, default_bellman_visitor() );
	//root_vertex(Vertex s)
	bellman_ford_shortest_paths( g, num_vertices(g), root_vertex( source ));
	distance_map = get( vertex_distance , g );
	cout << "End setWeightAndDistances" << endl;
}

void Batch::removeSourceNode( int toRemove )
{
	for( unsigned int i = 0 ; i < sources.size() ; i++ )
	{
		location l = sources[i];
		int comp = (l.y-1) * p_po->pom_x +l.x;
		if( comp == toRemove )
		{
			cout << "Source found, removing..." << endl;
			sources.erase(sources.begin()+i);
			break;
		}
	}
}

Batch::Batch( PO & po, int begin_frame_counter, vector<location> sources_in )
{
	p_po = &po;
	
	setBatchLimits( po, begin_frame_counter, batch_begin_frame, batch_num_of_frames );
	
	sources = sources_in;
	sources_backup = sources_in;
	source = p_po->vsource;
	unsigned int min_paths = sources.size();
	
	offset = log((1-po.pass_prob) / po.pass_prob);

	readFiles();

	vsink = vertices.size() + 1;
	batch_locations = vsink + 1;

	graph_t g;
	createGraph( g );
	po.first_graph = false;

	for( unsigned int s = 0 ; s < sources.size() ; s++ )
	{
		//put(edge_weight, g, x, v)
		location l = sources[s];
		int s_in = (l.y-1) * p_po->pom_x +l.x;
		remove_edge(p_po->vsource, s_in, g);
		add_edge(p_po->vsource, s_in, -10000, g);
	}

	g_original = g;

	/*int tempsource = 0;
	getSource();
	tempsource = source;
	source = p_po->vsource;*/
	//source = p_po->vsource;

	vector<int> path;
	getShortestPath( g, path );
	paths.push_back( path );
//	removeSourceNode( path[1] );

	cout << paths.size() << " path found." << endl;

	float costCurrent = pathsCost();
	float costPrev = 0.0;

	//vector_property_map<double> distances;
	//map<pair<int, int>, double> weights;

	//copy_weight_and_distance_map( g , weights , distance_map , distances );
	map<int,int> vertexPairs;
	for( unsigned int i = 1 ; i <= p_po->max_detections ; i++ ) 
	{
		if( i != 1 ) {
			costCurrent = pathsCost();
			if ( paths.size() > min_paths )
			{
				if( costCurrent >= costPrev ) 
				{
					//if( sources.size() == 0  )
					//{
						paths = prev_paths;
						break;
					//}
					//else
					//{
					//	getSource();
					//	tempsource = source;
					//	source = p_po->vsource;
					//}
				}
			}
			cout << paths.size() << " paths found." << endl; 
		}
		costPrev = costCurrent;
		prev_paths = paths;
		
		//graph_t	g = g_original;
		
		/*getSource();
		tempsource = source;
		source = p_po->vsource;*/
		//source = p_po->vsource;

		//if( tempsource != 0 )
		//{
		//	//put(edge_weight, g, x, v)
		//	add_edge(p_po->vsource, tempsource, -10000, g);
		//	tempsource = 0;
		//}

		//vector<int> dummy;
		//find_shortest_path_dag( g, dummy );

		transformEdgeCost( g ); //weights, distances, vertexPairs );

		extendGraphPath( g, path, vertexPairs );

		/*edge_iter ei, ei_end;
		tie( ei , ei_end ) = edges( g );
		for( ; ei != ei_end ; ei++ )
		{
			vertex_t s = boost::source( *ei , g );
			vertex_t t = boost::target( *ei , g );

			float cost = get( edge_weight , g , *ei );
			cout << s << " -> " << t << " : " << cost << endl;
		}*/

		//vector<int> path;
		getShortestPath( g, path );

		interlace_and_augment( path );
//		removeSourceNode( path[1] );
		//weights.clear();
		//copy_weight_and_distance_map( g , weights , distance_map , distances );
	}

	cout << endl; 
}